/*
 * mi_motor_drv.c
 *
 *  Created on: Dec 10, 2024
 *      Author: Jinming
 */
#include <string.h>
#include "mi_motor_drv.h"
#include "log.h"

#define TAG		"MiMotor"

#define miMotorArray2Int16(x)  (((int16_t)(*x << 8)) | *(x + 1))
#define miMotorArray2Uint16(x)  (((uint16_t)(*x << 8)) | *(x + 1))
#define miMotorArray2Int32(x)  (((int32_t)(*x << 24)) | ((int32_t)(*(x + 1)) << 16) | ((int32_t)(*(x + 2)) << 8) | *(x + 3))

typedef union
{
	struct
	{
		uint32_t addr : 8;				//电机CAN ID
		uint32_t data : 16;				//数据区2
		uint32_t type : 5;				//通信类型
		uint32_t reserve : 3;			//保留位
	};
	uint32_t id;
}MiCanId_t;

typedef union
{
	uint8_t data[4];
	float value;
}MiFloatFormat_t;

const osMutexAttr_t g_tMiMotorMutexAttr = {
  .name = "MiMotor"
};


int mi_motor_fault_feedback(MiMotorUnit_t *motor, CANMsgRx_t *frame);

static float miMotorUint2Float(int32_t x, int32_t x_min, int32_t x_max, float min, float max)
{
	float res;

	if(x > x_max)
	{
		x = x_max;
	}
	else if(x < x_min)
	{
		x = x_min;
	}

	res = (x - x_min) * (max - min) / (x_max - x_min) + min;

	return res;
}

uint32_t miMotorFloat2Uint(float x, float x_min, float x_max, int bits)
{
	float span = x_max - x_min;
	float offset = x_min;

	if(x > x_max)
	{
		x = x_max;
	}
	else if(x < x_min)
	{
		x = x_min;
	}
	return (uint32_t) ((x-offset) * ((float)((1<<bits)-1))/span);
}

static int setMiMotorInfo(MiMotorUnit_t *motor, uint32_t id, uint8_t *pData, uint8_t len, CANMsgRx_t *output)
{
	osStatus_t osStatus;
	CANMsgRx_t frame;

	osStatus = osMutexAcquire(motor->mutex, 10);
	if(osStatus)
	{
		return osStatus;
	}

	while(motor->receive(&frame, 0) == 0)			//有接收的数据未处理
	{
		mi_motor_fault_feedback(motor, &frame);
	}

	motor->send(id, pData, len);
	motor->clear();
	if(motor->receive(output, 10))
	{
		LOGE(TAG, "set Mi motor info error");
		osMutexRelease(motor->mutex);
		return -1;
	}
	else
	{
		mi_motor_fault_feedback(motor, &frame);
	}
	osMutexRelease(motor->mutex);

	return 0;
}

static int getMiMotorInfo(MiMotorUnit_t *motor, uint32_t id, CANMsgRx_t *output)
{
	osStatus_t osStatus;
	CANMsgRx_t frame;
	uint8_t aData[8];
	memset(aData, 0, sizeof(aData));

	osStatus = osMutexAcquire(motor->mutex, 10);
	if(osStatus)
	{
		return osStatus;
	}

	while(motor->receive(&frame, 0) == 0)			//有接收的数据未处理
	{
		mi_motor_fault_feedback(motor, &frame);
	}
	motor->send(id, aData, sizeof(aData));
	motor->clear();
	if(motor->receive(output, 10))
	{
		LOGE(TAG, "set Mi motor info error");
		osMutexRelease(motor->mutex);
		return -1;
	}
	else
	{
		mi_motor_fault_feedback(motor, &frame);
	}
	osMutexRelease(motor->mutex);

	return 0;
}

//通信类型2解析
static int mi_motor_feedback(MiMotorUnit_t *motor, CANMsgRx_t *frame)
{
	MiCanId_t tId;
	uint32_t curMotorId;

	tId.id = frame->extId;
	curMotorId = tId.data & 0xFF;
	if((tId.type != 2) || (tId.addr != SELF_CAN_ID) || (curMotorId != motor->canId))
	{
		return -1;
	}

//	log_buf(frame->data, sizeof(frame->data));
	motor->errCode &= 0xFF80;			//清理掉低7位，用来保存通信类型2的故障信息
	motor->errCode |= ((tId.data >> 16) & 0x3F);
	motor->pos = miMotorUint2Float(miMotorArray2Uint16(frame->data), 0, 65535, -4*PI, 4*PI);
	motor->speed = miMotorUint2Float(miMotorArray2Uint16((&(frame->data[2]))), 0, 65535, -30, 30);
	motor->torque = miMotorUint2Float(miMotorArray2Uint16((&(frame->data[4]))), 0, 65535, -12, 12);
	motor->temp = miMotorArray2Int16((&(frame->data[6])));
//	LOGD(TAG, "pos=%d\t%d\t%d\t%d", (int)(motor->pos*100), (int)(motor->speed*100), (int)(motor->torque*100), motor->temp);

	return 0;
}

int mi_motor_fault_feedback(MiMotorUnit_t *motor, CANMsgRx_t *frame)
{
	MiCanId_t tId;
	uint32_t temp1;
	uint16_t temp2;

	tId.id = frame->extId;
	if((tId.type != 2) || (tId.data != SELF_CAN_ID) || (tId.addr != motor->canId))
	{
		return -1;
	}
	temp1 = miMotorArray2Int32(frame->data);
	temp2 = (temp1 << 7) & 0x1F80;
	temp2 = ((temp1 & 0x80) > 0) ? (temp2 | 0x2000) : temp2;
	temp2 = ((temp1 & 0xF00) > 0) ? (temp2 | 0x4000) : temp2;
	temp2 = (temp1 & 0x8000) | temp2 ;

	motor->errCode &= 0x007F;		//清理掉高9位，用来存放故障反馈码
	motor->errCode |= temp2;

	return 0;
}

//获取电机的设备标志码，64字节
int mi_query_motor_dev_id(MiMotorUnit_t *motor, uint8_t *id)
{
	CANMsgRx_t frame;
	MiCanId_t tId;
	tId.id = 0;
	tId.addr = motor->canId;
	tId.data = SELF_CAN_ID;
	if(getMiMotorInfo(motor, tId.id, &frame))
	{
		return -1;
	}
	tId.id = frame.extId;
	if((tId.addr == 0xFE) && (tId.data == motor->canId))
	{
		memcpy(id, frame.data, sizeof(frame.data));
	}
	else
	{
		return -1;
	}

	return 0;
}

//电机是能运行
int mi_motor_enable(MiMotorUnit_t *motor)
{
	CANMsgRx_t frame;
	MiCanId_t tId;
	tId.id = 0;
	tId.addr = motor->canId;
	tId.data = SELF_CAN_ID;
	tId.type = 3;

	if(getMiMotorInfo(motor, tId.id, &frame))
	{
		return -1;
	}
	return mi_motor_feedback(motor, &frame);
}

//电机停止运行
int mi_motor_disenable(MiMotorUnit_t *motor)
{
	CANMsgRx_t frame;
	MiCanId_t tId;
	tId.id = 0;
	tId.addr = motor->canId;
	tId.data = SELF_CAN_ID;
	tId.type = 4;

	if(getMiMotorInfo(motor, tId.id, &frame))
	{
		return -1;
	}
	return mi_motor_feedback(motor, &frame);
}

//设置电机的机械零位
int mi_motor_zero_position_set(MiMotorUnit_t *motor)
{
	CANMsgRx_t frame;
	MiCanId_t tId;
	uint8_t aData[8];

	tId.id = 0;
	tId.addr = motor->canId;
	tId.data = SELF_CAN_ID;
	tId.type = 6;
	memset(aData, 0, sizeof(aData));
	aData[0] = 1;

	if(setMiMotorInfo(motor, tId.id, aData, sizeof(aData), &frame))
	{
		return -1;
	}
	return mi_motor_feedback(motor, &frame);
}

//读取单个参数
int mi_motor_read_param(MiMotorUnit_t *motor, uint16_t index, void *output)
{
	CANMsgRx_t frame;
	MiCanId_t tId;
	uint8_t aData[8];
	MiFloatFormat_t tTranslate;
	uint16_t recIndex;

	tId.id = 0;
	tId.addr = motor->canId;
	tId.data = SELF_CAN_ID;
	tId.type = 17;
	memset(aData, 0, sizeof(aData));
	aData[0] = index;
	aData[1] = index >> 8;

	if(setMiMotorInfo(motor, tId.id, aData, sizeof(aData), &frame))
	{
		LOGD(TAG, "1");
		return -1;
	}

	tId.id = frame.extId;
	if((tId.type != 17) || ((tId.data & 0xff) != motor->canId) || (tId.addr != SELF_CAN_ID))
	{
		LOGD(TAG, "2");
		return -1;
	}

	recIndex = ((int16_t)(frame.data[1]) << 8) | frame.data[0];
	if(recIndex != index)
	{
		LOGD(TAG, "3");
		return -1;
	}

	switch(index)
	{
	case MI_MOTOR_PARAM_RUM_MODE:
		motor->curRunMode = frame.data[4];
		*((uint8_t *)output) = frame.data[4];
		break;

	case MI_MOTOR_PARAM_IQ_REF:
	case MI_MOTOR_PARAM_SPD_REF:
	case MI_MOTOR_PARAM_LIMIT_TORQUE:
	case MI_MOTOR_PARAM_CUR_KP:
	case MI_MOTOR_PARAM_CUR_KI:
	case MI_MOTOR_PARAM_CUR_FILT_GAIN:
	case MI_MOTOR_PARAM_LOC_REF:
	case MI_MOTOR_PARAM_LIMIT_SPD:
	case MI_MOTOR_PARAM_LIMIT_CUR:
	case MI_MOTOR_PARAM_MECH_POS:
	case MI_MOTOR_PARAM_IQF:
	case MI_MOTOR_PARAM_MECH_VEL:
	case MI_MOTOR_PARAM_VBUS:
	case MI_MOTOR_PARAM_LOC_KP:
	case MI_MOTOR_PARAM_SPD_KP:
	case MI_MOTOR_PARAM_SPD_KI:
		for(uint8_t i = 0; i < 4; i++)
		{
			tTranslate.data[i] = frame.data[4+i];		//小端模式
		}
		*((float *)output) = tTranslate.value;
		break;

	case MI_MOTOR_PARAM_ROTATION:
		*((int16_t *)output) = miMotorArray2Int16(&frame.data[4]);
		break;

	default:
		break;
	}

	return 0;
}

//读取单个参数
int mi_motor_write_param(MiMotorUnit_t *motor, uint16_t index, void *input)
{
	CANMsgRx_t frame;
	MiCanId_t tId;
	uint8_t aData[8];
	MiFloatFormat_t tTranslate;

	tId.id = 0;
	tId.addr = motor->canId;
	tId.data = SELF_CAN_ID;
	tId.type = 18;
	memset(aData, 0, sizeof(aData));

	switch(index)
	{
	case MI_MOTOR_PARAM_RUM_MODE:
		aData[4] = *((uint8_t *)input);
		break;

	case MI_MOTOR_PARAM_IQ_REF:
	case MI_MOTOR_PARAM_SPD_REF:
	case MI_MOTOR_PARAM_LIMIT_TORQUE:
	case MI_MOTOR_PARAM_CUR_KP:
	case MI_MOTOR_PARAM_CUR_KI:
	case MI_MOTOR_PARAM_CUR_FILT_GAIN:
	case MI_MOTOR_PARAM_LOC_REF:
	case MI_MOTOR_PARAM_LIMIT_SPD:
	case MI_MOTOR_PARAM_LIMIT_CUR:
	case MI_MOTOR_PARAM_LOC_KP:
	case MI_MOTOR_PARAM_SPD_KP:
	case MI_MOTOR_PARAM_SPD_KI:
		tTranslate.value = *((float *)input);
		for(uint8_t i = 0; i < 4; i++)
		{
			aData[4 + i] = tTranslate.data[i];		//小端模式
		}
		break;

	case MI_MOTOR_PARAM_ROTATION:
		aData[4] = *((int16_t *)input);
		aData[5] = (*((int16_t *)input)) >> 8;
		break;

	default:
		return -1;
	}

	aData[0] = index;
	aData[1] = index >> 8;

	if(setMiMotorInfo(motor, tId.id, aData, sizeof(aData), &frame))
	{
		return -1;
	}
	return mi_motor_feedback(motor, &frame);
}

int mi_motor_init(MiMotorUnit_t *motor, uint32_t id, MiMotorAttr_t *attr)
{
	motor->canId = id;
	motor->curRunMode = MI_MOTOR_MODE_IDLE;
    memcpy(&motor->tMotorAttr, attr, sizeof(MiMotorAttr_t));
	motor->mutex = osMutexNew(&g_tMiMotorMutexAttr);
	if(motor->mutex == NULL)
	{
		LOGE(TAG, "Mi motor %ld mutex create error", id);
		return -1;
	}

    return 0;
}

MI_MOTOR_MODE_e mi_motor_curMode_get(MiMotorUnit_t *motor)
{
	return motor->curRunMode;
}

int mi_motor_mode_change(MiMotorUnit_t *motor, uint16_t mode)
{
	int ret;
	float tmp;

    if(motor->curRunMode == mode)			//已经处于该模式了，不需要切换控制模式
    {
    	return 0;
    }

    mi_motor_disenable(motor);
    if(mode == MI_MOTOR_MODE_POS)
    {
    	ret = mi_motor_write_param(motor, MI_MOTOR_PARAM_RUM_MODE, &mode);
    	if(ret == 0)
    	{
    		tmp = motor->tMotorAttr.maxSpeed / 60 * PI2;
    		mi_motor_enable(motor);
    		mi_motor_write_param(motor, MI_MOTOR_PARAM_LIMIT_SPD, &tmp);
    		motor->curRunMode = MI_MOTOR_MODE_POS;
    		LOGD(TAG, "change to MI_MOTOR_MODE_POS");
    	}
    	else
    	{
    		LOGD(TAG, "pos mode change err");
    	}
    }
    else if(mode == MI_MOTOR_MODE_RPM)
    {
    	ret = mi_motor_write_param(motor, MI_MOTOR_PARAM_RUM_MODE, &mode);
    	if(ret == 0)
    	{
    		mi_motor_enable(motor);
    		mi_motor_write_param(motor, MI_MOTOR_PARAM_LIMIT_CUR, &motor->tMotorAttr.maxCurrent);
    		motor->curRunMode = MI_MOTOR_MODE_RPM;
    		LOGD(TAG, "change to MI_MOTOR_MODE_RPM");
    	}
    	else
    	{
    		LOGD(TAG, "rpm mode change err");
    	}
    }
    else if(mode == MI_MOTOR_MODE_TORQUE)
    {
    	ret = mi_motor_write_param(motor, MI_MOTOR_PARAM_RUM_MODE, &mode);
    	if(ret == 0)
    	{
    		mi_motor_enable(motor);
    		motor->curRunMode = MI_MOTOR_MODE_TORQUE;
    		LOGD(TAG, "change to MI_MOTOR_MODE_TORQUE");
    	}
    	else
    	{
    		LOGD(TAG, "torque mode change err");
    	}
    }
    else if(mode == MI_MOTOR_MODE_RUN_CTRL)
    {
//    	temp = 0;
//    	mi_motor_write_param(motor, MI_MOTOR_PARAM_IQ_REF, &temp);
    	ret = mi_motor_write_param(motor, MI_MOTOR_PARAM_RUM_MODE, &mode);
    	if(ret == 0)
    	{
    		mi_motor_enable(motor);
    		motor->curRunMode = MI_MOTOR_MODE_RUN_CTRL;
    		LOGD(TAG, "change to MI_MOTOR_MODE_RUN_CTRL");
    	}
    	else
    	{
    		LOGD(TAG, "run ctrl mode change err");
    	}
    }
    else if(mode == MI_MOTOR_MODE_IDLE)		//关停电机即可
    {
    	motor->curRunMode = MI_MOTOR_MODE_IDLE;
    	LOGD(TAG, "change to MI_MOTOR_MODE_IDLE");
    }

    return ret;
}

//设置电机的力矩，单位：Nm
int mi_motor_torque_set(MiMotorUnit_t *motor, float Nm)
{
	float current = Nm / motor->tMotorAttr.torqueCoeff;
	return mi_motor_write_param(motor, MI_MOTOR_PARAM_IQ_REF, &current);
}

//获取电机当前电流，单位：A
int mi_motor_current_get(MiMotorUnit_t *motor, float *ampere)
{
	*ampere = motor->torque / motor->tMotorAttr.torqueCoeff;

	return 0;
}

//速度模式下设置目标速度，单位rpm
int mi_motor_speed_set(MiMotorUnit_t *motor, float rpm)
{
	float rads = rpm * PI2 / 60.0f;   //单位转换,rpm->rad/s
	return mi_motor_write_param(motor, MI_MOTOR_PARAM_SPD_REF, &rads);
}

//获取电机实际转速，单位RPM
int mi_motor_speed_get(MiMotorUnit_t *motor, float *speed)
{
	*speed = motor->speed / PI2 * 60;
//	LOGD(TAG, "speed=%d", (int)((*speed) * 10));
	return 0;
}

//设置电机的目标位置，单位：度
int mi_motor_position_set(MiMotorUnit_t *motor, float deg)
{
	float rad = deg * PI2 / 360.0f;   //单位转换,deg->rad
	return mi_motor_write_param(motor, MI_MOTOR_PARAM_LOC_REF, &rad);
}

//获取电机当前位置，单位：度
int mi_motor_position_get(MiMotorUnit_t *motor, float *pos)
{
	*pos = motor->pos / PI2 * 360;
	return 0;
}

//获取电流Kp
int mi_motor_current_kp_get(MiMotorUnit_t *motor, float *kp)
{
	if(mi_motor_read_param(motor, MI_MOTOR_PARAM_CUR_KP, kp))
	{
		return -1;
	}

	return 0;
}

//设置电机最大力矩，单位：Nm
int mi_motor_limit_torque_set(MiMotorUnit_t *motor, float torque)
{
	return mi_motor_write_param(motor, MI_MOTOR_PARAM_LIMIT_TORQUE, &torque);
}

//获取电机当前温度，单位：摄氏度
int mi_motor_temperature_get(MiMotorUnit_t *motor, int16_t *temp)
{
	*temp = motor->temp;
	return 0;
}

//运控模式参数设置;
//pos单位:度;
//speed单位:RPM;
//kp:0.0~500.0;
//ki:0.0~5.0
int mi_motor_runctrl_param_set(MiMotorUnit_t *motor, float pos, float speed, float kp, float ki)
{
	CANMsgRx_t frame;
	MiCanId_t tId;
	uint8_t aData[8];
	uint16_t tmp;
	float trans;

	tId.id = 0;
	tId.addr = motor->canId;
	tId.data = SELF_CAN_ID;
	tId.type = 1;
	memset(aData, 0, sizeof(aData));

	trans = pos * PI2 / 360;
	tmp = miMotorFloat2Uint(trans, -12.566370616f, 12.566370616f, 16);
	aData[0] = tmp >> 8;
	aData[1] = tmp;

	trans = speed * PI2 / 60;
	tmp = miMotorFloat2Uint(speed, -30.0f, 30.0f, 16);
	aData[2] = tmp >> 8;
	aData[3] = tmp;

	tmp = miMotorFloat2Uint(kp, 0.0f, 500.0f, 16);
	aData[4] = tmp >> 8;
	aData[5] = tmp;

	tmp = miMotorFloat2Uint(ki, 0.0f, 5.0f, 16);
	aData[6] = tmp >> 8;
	aData[7] = tmp;

	if(setMiMotorInfo(motor, tId.id, aData, sizeof(aData), &frame))
	{
		return -1;
	}

	return mi_motor_feedback(motor, &frame);
}
